
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       ahrs.c
  * @author     baiyang
  * @date       2021-10-31
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include <rtthread.h>
#include <rtdevice.h>

#include "ahrs.h"
#include <ahrs/ahrs_ekf.h>
#include <ahrs/ahrs_sensor.h>
#include <common/grapilot.h>
#include <common/time/gp_time.h>
#include <board_config/borad_config.h>
/*-----------------------------------macro------------------------------------*/
#define EVENT_AHRS_FAST_LOOP        (1<<0)
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void ahrs_nav_run();
/*----------------------------------variable----------------------------------*/
static struct rt_timer timer_ahrs_ekf;
static struct rt_event event_ahrs_ekf;

static char thread_ahrs_ekf_stack[1024*20];
struct rt_thread thread_ahrs_ekf_handle;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
static void timer_ahrs_update(void* parameter)
{
    rt_event_send(&event_ahrs_ekf, EVENT_AHRS_FAST_LOOP);
}

void navigation_entry(void *parameter)
{
    rt_err_t res;
    rt_uint32_t recv_set = 0;
    rt_uint32_t wait_set = EVENT_AHRS_FAST_LOOP;

    /* create event */
    res = rt_event_init(&event_ahrs_ekf, "ahrs_ekf", RT_IPC_FLAG_FIFO);

    /* register timer event */
    rt_timer_init(&timer_ahrs_ekf, "ahrs_ekf",
                    timer_ahrs_update,
                    RT_NULL,
                    rt_tick_from_millisecond(1),
                    RT_TIMER_FLAG_PERIODIC | RT_TIMER_FLAG_HARD_TIMER);
    rt_timer_start(&timer_ahrs_ekf);

    while(1)
    {
        res = rt_event_recv(&event_ahrs_ekf, wait_set, RT_EVENT_FLAG_OR | RT_EVENT_FLAG_CLEAR, 
                                RT_WAITING_FOREVER, &recv_set);
        
        if(res == RT_EOK){
            if(recv_set & EVENT_AHRS_FAST_LOOP){
                ahrs_nav_run();
            }
        }
    }
}

static void ahrs_nav_init()
{
    ahrs_sensor_init(1000/PERIOD_MS_100HZ);
    ahrs_ekf_init();
    ahrs_gcs_init();
}

static void ahrs_nav_run()
{
    uint32_t now_ms = time_millis();

    TIMETAG_CHECK_EXECUTE2(ahrs_gcs, PERIOD_MS_100HZ, now_ms, ahrs_gcs_update();)

    ahrs_sensor_update(now_ms);

    TIMETAG_CHECK_EXECUTE2(ahrs_ekf, PERIOD_MS_100HZ, now_ms, ahrs_ekf_update();)
}

void task_ahrs_nav_init()
{
    rt_err_t res;

    ahrs_nav_init();

    res = rt_thread_init(&thread_ahrs_ekf_handle,
                           "ahrs_ekf",
                           navigation_entry,
                           RT_NULL,
                           &thread_ahrs_ekf_stack[0],
                           sizeof(thread_ahrs_ekf_stack),PRIORITY_AHRS,10);
    if (res == RT_EOK)
        rt_thread_startup(&thread_ahrs_ekf_handle);

    brd_set_vehicle_init_stage(INIT_STAGE_AHRS_NAV);
}

/*------------------------------------test------------------------------------*/


